import pysoem
import time
from multiprocessing import Process, LifoQueue
import numpy as np

def read_process(queue):
    master = pysoem.Master()
    master.open('eth1')
    master.config_dc()

    if master.config_init() > 0:
        master.config_map()
        
        for slave in master.slaves:
            print(f"slave name is {slave.name}")
            print(f"slave state is {slave.state}")
            print(f"slave id is {slave.id}")

        master.state_check(pysoem.SAFEOP_STATE, 50000)
        master.state_check(pysoem.OP_STATE, 50000)
        master.write_state()
        master.in_op = True
        try:
            while True:
                master.send_processdata()
                master.receive_processdata(2000)
                slave = master.slaves[0]
                
                # 假设 NET_1 到 NET_16 每个数据占用 4 字节 (32 位)
                data_offset = 0
                data_length = 4
                
                force_list = np.zeros(16, dtype=int)
                for i in range(1, 17):
                    data_end = data_offset + data_length
                    value = int.from_bytes(slave.input[data_offset:data_end], byteorder='little', signed=True)
                    force_list[i-1] = value
                    # print(f"NET_{i}: {value}")
                    data_offset += data_length
                
                
                queue.put(force_list)
                print(f"force_list from test.py: {force_list}")
                
                # 添加一个小延迟以避免 CPU 占用过高
                time.sleep(0.0006)
                

        except KeyboardInterrupt:
            print("KeyboardInterrupt")
        finally:
            master.state = pysoem.INIT_STATE
            master.write_state()
            master.close()


if __name__ == "__main__":
    queue = LifoQueue(maxsize=1)
    process = Process(target=read_process, args=(queue,))
    process.start()